﻿using AntMovement.Core.Entity;
using AntMovement.Core.Hardware;
using AntMovement.Core.IParameter;
using AntMovement.Core.Model;
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using System.Xml.Linq;

namespace AntMovement.Core.Logic
{
    public class AxisMotion
    {
        private Ton _moveTimeOutAlarm = new Ton();
        private Ton _homeTimeOutAlarm = new Ton();
        private Ton _ton1 = new Ton();
        private Ton _ton2 = new Ton();
        private IAxis _axisMotion;
        private AxisStatus _axisStatus;
        private AxisParameter _axisParameter;
        private bool _stopMotion = false;
        private bool _arrivalLocation;
        private bool _homeComplete = false;
        #region 上升沿
        private bool _up = false, _down = false, _move = false, _alm = false, _emg = false;
        #endregion
        #region 超时报警

        private bool _alarmMove = false, _alarmHome = false;

        /// <summary>
        /// 运动超时
        /// </summary>
        public bool AlarmMove { get => _alarmMove; }

        /// <summary>
        /// 回原超时
        /// </summary>
        public bool AlarmHome { get => _alarmHome; }

        /// <summary>
        /// 伺服报警
        /// </summary>
        public bool Alm { get => _axisStatus.Alm; }
        
        /// <summary>
        /// 伺服急停
        /// </summary>
        public bool EMG { get => _axisStatus.EMG; }

        #endregion
        public bool StopMotion { get => _axisStatus?.SportStatus ?? true; }
        /// <summary>
        /// 回原完成
        /// </summary>
        public bool HomeComplete { get => _homeComplete; }
        public bool ArrivalLocation { get => _arrivalLocation && !_axisStatus.SportStatus; }
        public string Text { get => _text; }
        private string _text;
        private ILog _log;
        public AxisMotion(IAxisParameter axisp, string text, ILog log)
        {
            _text = text;
            _axisMotion = axisp.GetAxis(text);
            _axisStatus = _axisMotion.GetAxisStatus();
            _axisParameter = _axisMotion.GetAxisParameter();
            _axisStatus.IsHome = false;
            _log = log;
        }

        /// <summary>
        /// 连续运动
        /// </summary>
        /// <param name="up">正方向</param>
        /// <param name="down">负方向</param>
        public void Continuous(bool up, bool down)
        {
            if (Detection())
            {
                return;
            }
            if (up && !_up && !_axisStatus.SportStatus)
            {
                _up = true;
                _axisMotion.ContinuousMotion(true);
            }
            if (down && !_down && !_axisStatus.SportStatus)
            {
                _down = true;
                _axisMotion.ContinuousMotion(false);
            }
            if ((!up && _up) || !down && _down)
            {
                _up = false;
                _down = false;
                Stop(1);
            }
        }

        private bool Detection()
        { 
            if (_axisStatus.EMG && !_emg)
            {
                _log.Error("伺服", $"[{_text}] 伺服报警");
                _emg = true;
                return true;
            }
            if (_axisStatus.Alm && !_alm)
            {
                _log.Error("急停", $"[{_text}] 触发急停");
                _alm = true;
                return true;
            }
            return false;
        }

        /// <summary>
        /// 绝对运动
        /// </summary>
        /// <param name="pulse">脉冲数</param>
        /// <param name="timeout">超时报警</param>
        /// <param name="difference">目标脉冲差范围</param>
        public void Move(long pulse, long timeout, long difference = 10, bool bl = true)
        {
            if (Detection())
            { 
                return;
            }
            if ((bl && Math.Abs(pulse - _axisStatus.CurrentPostion) <= difference)
                || (!bl && Math.Abs(pulse - _axisStatus.CurrentPostion) <= difference && Math.Abs(pulse - _axisStatus.EqPostion) <= difference)
                && !_axisStatus.SportStatus)
            {
                _arrivalLocation = true;
                _moveTimeOutAlarm.Time(timeout, false);
                _move = false;
            }
            else
            {
                _arrivalLocation = false;
                _moveTimeOutAlarm.Time(timeout, true);
                _alarmMove = _moveTimeOutAlarm.Out;
                if (_alarmMove)
                {
                    _log.Error("轴", $"[{_text}]运动超时...");
                }
                if (!_axisStatus.SportStatus && !_move && !_alarmMove)
                {
                    _axisMotion.SetProfile();
                    _axisMotion.Move(pulse, 1, 2);
                    _move = true;
                }
            }
        }

        public void Resetting()
        {
            _move = false;
            _alarmHome = false;
            _alarmMove = false;
            _moveTimeOutAlarm.Time(100, false);
            _homeTimeOutAlarm.Time(100, false);
            _homeMove = false;
            _emg = false;
            _alm = false;
        }

        private bool _CustomZeroMode;
        private bool _homeMove;
        private bool _FuncCustomZero;
        public void Home(long timeout = 60000)
        {
            if (Detection())
            {
                return;
            }
            if (!_FuncCustomZero && !FuncCustomZero())
            {
                return;
            }
            _FuncCustomZero = true;
            _homeTimeOutAlarm.Time(timeout, !_homeComplete);
            if (_homeTimeOutAlarm.Out)
            {
                _log.Error("轴", $"[{_text}] 回原超时!!!");
                _alarmHome = true;
                return;
            }
            if (_homeMove && !_axisStatus.SportStatus && _axisStatus.Homing)
            {
                _homeComplete = true;
            }
            if (!_homeMove)
            {
                _axisMotion.HomeMove();
                _homeMove = true;
                _homeComplete = false;
            }
            _axisStatus.IsHome = _homeComplete;
        }

        #region 回原模式
        private int _zreoMode = 0;
        private bool _homeCustom = false;
        #endregion
        private bool FuncCustomZero()
        {
            if (_axisParameter.CustomZeroMode != Enum.CustomZeroModeType.None)
            {
                if (_axisParameter.CustomZeroMode == Enum.CustomZeroModeType.负极限碰原点回原)
                {
                    switch (_zreoMode)
                    {
                        case 0:
                            ZeroContinuous(false, !_axisStatus.El_Right && !_axisStatus.ORG);
                            _ton1.Time(100, !_axisStatus.SportStatus && _axisStatus.El_Right);
                            _ton2.Time(100, !_axisStatus.SportStatus && _axisStatus.ORG);
                            if (_ton1.Out)
                            {
                                _ton1.Time(100, false);
                                _zreoMode = 2;
                            }
                            else if (_ton2.Out)
                            {
                                _ton2.Time(100, false);
                                _zreoMode = 1;
                            }
                            break;
                        case 1:
                            ZeroContinuous(false, _axisStatus.ORG);
                            _ton1.Time(100, !_axisStatus.SportStatus && !_axisStatus.ORG);
                            if (_ton1.Out)
                            {
                                _ton1.Time(100, false);
                                return true;
                            }
                            break;
                        case 2:
                            if (!_homeCustom)
                            {
                                ZeroContinuous(!_axisStatus.ORG, false);
                                _ton1.Time(100, !_axisStatus.SportStatus && _axisStatus.ORG);
                                if (_ton1.Out)
                                {
                                    _ton1.Time(100, false);
                                    _homeCustom = true;
                                }
                            }
                            else if (_homeCustom) 
                            {
                                ZeroContinuous(false, _axisStatus.ORG);
                                _ton1.Time(100, !_axisStatus.SportStatus && !_axisStatus.ORG);
                                if (_ton1.Out)
                                {
                                    _ton1.Time(100, false);
                                    return true;
                                }
                            }
                            break;
                    }
                }
                if (_axisParameter.CustomZeroMode == Enum.CustomZeroModeType.正极限碰原点回原)
                {
                    switch (_zreoMode)
                    {
                        case 0:
                            ZeroContinuous(!_axisStatus.El_Left && !_axisStatus.ORG, false);
                            _ton1.Time(100, !_axisStatus.SportStatus && _axisStatus.El_Left);
                            _ton2.Time(100, !_axisStatus.SportStatus && _axisStatus.ORG);
                            if (_ton1.Out)
                            {
                                //正方向走到极限，开始往负方向走遇到原点，上升沿标记，往正方向走，原点信号消失，开始回原
                                _ton1.Time(100, false);
                                _zreoMode = 2;
                            }
                            else if (_ton2.Out)
                            {
                                //1 正方向走遇到原点，上升沿标记，继续往正方向走，原点消失，开始回原
                                _ton2.Time(100, false);
                                _zreoMode = 1;
                            }
                            break;
                        case 1:
                            ZeroContinuous(_axisStatus.ORG, false);
                            _ton1.Time(100, !_axisStatus.SportStatus && !_axisStatus.ORG);
                            if (_ton1.Out)
                            {
                                _ton1.Time(100, false);
                                return true;
                            }
                            break;
                        case 2:
                            if (!_homeCustom)
                            {
                                ZeroContinuous(false, !_axisStatus.ORG);
                                _ton1.Time(100, !_axisStatus.SportStatus && _axisStatus.ORG);
                                if (_ton1.Out)
                                {
                                    _ton1.Time(100, false);
                                    _homeCustom = true;
                                }
                            }
                            else if (_homeCustom)
                            {
                                ZeroContinuous(_axisStatus.ORG, false);
                                _ton1.Time(100, !_axisStatus.SportStatus && !_axisStatus.ORG);
                                if (_ton1.Out)
                                {
                                    _ton1.Time(100, false);
                                    return true;
                                }
                            }
                            break;
                    }
                }
                if (_axisParameter.CustomZeroMode == Enum.CustomZeroModeType.负极限原点正方向回原)
                {
                    switch (_zreoMode)
                    {
                        case 0:
                            ZeroContinuous(false, !_axisStatus.El_Right && !_axisStatus.ORG);
                            _ton1.Time(100, !_axisStatus.SportStatus && _axisStatus.El_Right);
                            _ton2.Time(100, !_axisStatus.SportStatus && _axisStatus.ORG);
                            if (_ton1.Out)
                            {
                                //2 负方向走到极限，开始往正方向走遇到原点，上升沿标记，往正方向走，原点信号消失，开始回原
                                _ton1.Time(100, false);
                                _zreoMode = 2;
                            }
                            else if (_ton2.Out)
                            {
                                //1 负方向走遇到原点，上升沿标记，继续往正方向走，原点消失，开始回原
                                _ton2.Time(100, false);
                                _zreoMode = 1;
                            }
                            break;
                        case 1:
                            ZeroContinuous(_axisStatus.ORG, false);
                            _ton1.Time(100, !_axisStatus.SportStatus && !_axisStatus.ORG);
                            if (_ton1.Out)
                            {
                                _ton1.Time(100, false);
                                return true;
                            }
                            break;
                        case 2:
                            if (!_homeCustom)
                            {
                                ZeroContinuous(!_axisStatus.ORG, false);
                                _ton1.Time(100, !_axisStatus.SportStatus && _axisStatus.ORG);
                                if (_ton1.Out)
                                {
                                    _ton1.Time(100, false);
                                    _homeCustom = true;
                                }
                            }
                            else if (_homeCustom)
                            {
                                ZeroContinuous(_axisStatus.ORG, false);
                                _ton1.Time(100, !_axisStatus.SportStatus && !_axisStatus.ORG);
                                if (_ton1.Out)
                                {
                                    _ton1.Time(100, false);
                                    return true;
                                }
                            }
                            break;
                    }
                }
                if (_axisParameter.CustomZeroMode == Enum.CustomZeroModeType.正极限原点负方向回原)
                {
                    switch (_zreoMode)
                    {
                        case 0:
                            ZeroContinuous(!_axisStatus.El_Left && !_axisStatus.ORG, false);
                            _ton1.Time(100, !_axisStatus.SportStatus && _axisStatus.El_Left);
                            _ton2.Time(100, !_axisStatus.SportStatus && _axisStatus.ORG);
                            if (_ton1.Out)
                            {
                                //2 正方向走到极限，开始往负方向走遇到原点，上升沿标记，往负方向走，原点信号消失，开始回原
                                _ton1.Time(100, false);
                                _zreoMode = 2;
                            }
                            else if (_ton2.Out)
                            {
                                //1 正方向走遇到原点，上升沿标记，继续往负方向走，原点消失，开始回原
                                _ton2.Time(100, false);
                                _zreoMode = 1;
                            }
                            break;
                        case 1:
                            ZeroContinuous(false, _axisStatus.ORG);
                            _ton1.Time(100, !_axisStatus.SportStatus && !_axisStatus.ORG);
                            if (_ton1.Out)
                            {
                                _ton1.Time(100, false);
                                return true;
                            }
                            break;
                        case 2:
                            if (!_homeCustom)
                            {
                                ZeroContinuous(false, !_axisStatus.ORG);
                                _ton1.Time(100, !_axisStatus.SportStatus && _axisStatus.ORG);
                                if (_ton1.Out)
                                {
                                    _ton1.Time(100, false);
                                    _homeCustom = true;
                                }
                            }
                            else if (_homeCustom)
                            {
                                ZeroContinuous(false, _axisStatus.ORG);
                                _ton1.Time(100, !_axisStatus.SportStatus && !_axisStatus.ORG);
                                if (_ton1.Out)
                                {
                                    _ton1.Time(100, false);
                                    return true;
                                }
                            }
                            break;
                    }
                }
            }
            else
            {
                return true;
            }
            return false;
        }
        private bool _ZeroUp = false, _ZeroDown = false;

        private void ZeroContinuous(bool up, bool down)
        {
            if (Detection())
            {
                return;
            }
            if (up && !_ZeroUp && !_axisStatus.SportStatus)
            {
                _ZeroUp = true;
                _axisMotion.ContinuousMotion(true);
            }
            if (down && !_ZeroDown && !_axisStatus.SportStatus)
            {
                _ZeroDown = true;
                _axisMotion.ContinuousMotion(false);
            }
            if ((!up && _ZeroUp) || !down && _ZeroDown)
            {
                _ZeroUp = false;
                _ZeroDown = false;
                Stop(1);
            }
        }

        private bool _servo = false;

        public void SetServo(bool enable)
        {
            string error = default;
            if (!_servo && !enable)
            {
                error = _axisMotion.SetServo(false);
                _servo = true;
            }
            else if (enable && _servo)
            {
                _servo = false;
                error = _axisMotion.SetServo(true);
            }
        }

        public void Stop(int emgStop)
        {
            _axisMotion.Stop(emgStop);
        }

        public void ClearEnc()
        {
            _axisMotion.ClearCEnc();
        }

        public void ClearPos()
        {
            _axisMotion.ClearCPos();
        }
    }
}
